#include "global_smoother.h"

void GlobalSmoother::processAndPublishData()
{
  std::vector<double> map_1_x, map_1_y;
  std::ifstream file(csv_file_path);
  std::string line;

  while (std::getline(file, line)) {
    std::stringstream linestream(line);
    std::string cell;
    std::vector<std::string> parsedRow;
    while (std::getline(linestream, cell, ',')) {
      parsedRow.push_back(cell);
    }
    // 只处理前两列数据
    if (parsedRow.size() >= 2) {
      map_1_x.push_back(std::stod(parsedRow[0]));
      map_1_y.push_back(std::stod(parsedRow[1]));
    }
  }
  // 初始化 hdmap_way_points
  hdmap_way_points = Eigen::MatrixXd::Zero(map_1_x.size(), 3);
  for (size_t i = 0; i < map_1_x.size(); ++i) {
    hdmap_way_points(i, 0) = map_1_x[i];
    hdmap_way_points(i, 1) = map_1_y[i];
    hdmap_way_points(i, 2) = 0; // 填充0作为第三列数据
  }

  Gl.global_refer(hdmap_way_points);
  hasProcessed = true;
}

void GlobalSmoother::Odom(const nav_msgs::Odometry &msg)
{
  car_odom_ = msg;
  global_odom_x  =  car_odom_.pose.pose.position.x ;
  global_odom_y  =  car_odom_.pose.pose.position.y ;
  global_odom_yaw  =  tf::getYaw(car_odom_.pose.pose.orientation) ;
}

void GlobalSmoother::Glob_receive(const geometry_msgs::PoseStamped &msg)
{
  is_glob_receive = true;
}

void GlobalSmoother::startThread() 
{
  ros::Rate loop_rate(10);
  while (nh.ok()) {
    if(is_glob_receive == true){
      processAndPublishData();
      if (hasProcessed) {
          break;
      }
      is_glob_receive = false;
    }
    ros::spinOnce();
    loop_rate.sleep();
  }
}

GlobalSmoother::GlobalSmoother(){

  // ros::param::get("csv_file_path", csv_file_path);
  csv_file_path = "/home/x/smoother_frenet_paths/src/global_smoother/path/aaa.csv";
  std::cout << YELLOW << "csv_file_path: " << csv_file_path << RESET << std::endl;
  if (csv_file_path.empty()) {
    ROS_ERROR("Failed to get parameter 'csv_file_path' parameter. Ensure it is set correctly in the launch file.");
    return;
  }
  ros::Rate loop_rate(10);
  hasProcessed = false;

  odom_ = nh.subscribe("/odom", 10, &GlobalSmoother::Odom, this);
  move_base_simple_sub_ = nh.subscribe("/move_base_simple/goal", 10, &GlobalSmoother::Glob_receive, this);
  task_thread_ = new boost::thread(boost::bind(&GlobalSmoother::startThread, this));
  
}

GlobalSmoother::~GlobalSmoother(){
  task_thread_->detach();
  delete task_thread_;
}
